/*   The MIT License
 *   
 *   Carina Engine
 *   Copyright (c) 2009 2010 2011 2012 Zdravko Velinov
 *   
 *   Permission is hereby granted, free of charge, to any person obtaining a copy
 *   of this software and associated documentation files (the "Software"), to deal
 *   in the Software without restriction, including without limitation the rights
 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *   copies of the Software, and to permit persons to whom the Software is
 *   furnished to do so, subject to the following conditions:
 *
 *   The above copyright notice and this permission notice shall be included in
 *   all copies or substantial portions of the Software.
 *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 *   THE SOFTWARE.
 */

#define MATH_CC

#include "carina/math.hh"
#include "carina/common/exception.hh"

#include <iomanip>
#include <cstring>
#include <cmath>
#include <cfloat>

namespace Carina
{
#ifdef _MSC_VER
unsigned log2(unsigned x)
{
    unsigned c = 0;
    while(x>>=1)
        ++c;
    return c;
}
#endif

unsigned pow_of_2(unsigned num)
{
    assert(num < (unsigned)1 << 31);
    if(num < 2)
        return 2;

    --num;
    num |= (num >> 1);
    num |= (num >> 2);
    num |= (num >> 4);
    num |= (num >> 8);
    num |= (num >> 16);
    return ++num;
}

bool is_not_pow_2_1(unsigned n)
{
    return ((n - 1) & (n - 2)) != 0;
}

bool is_not_pow_2(unsigned n)
{
    return (n & (n - 1)) != 0;
}

Vector2::Vector2(const Vector2& v)
{
    set(v.coordinate.x, v.coordinate.y);
}

Vector2::Vector2(const Vector3& v)
{
    set(v.coordinate.x, v.coordinate.z);
}
Vector2::Vector2(const Vector4& v)
{
    set(v.coordinate.x, v.coordinate.z);
}

Vector3::Vector3(const Vector3& v)
{
    set(v.coordinate.x, v.coordinate.y, v.coordinate.z);
}

Vector3::Vector3(const Vector4& v)
{
    set(v.coordinate.x / v.coordinate.w,
        v.coordinate.y / v.coordinate.w,
        v.coordinate.z / v.coordinate.w);
}

float Vector3::dot(const Vector4& vec) const
{
    return coordinate.x*vec.coordinate.x +
           coordinate.y*vec.coordinate.y +
           coordinate.z*vec.coordinate.z +
           vec.coordinate.w;
}

Vector4::Vector4(const Vector4& v)
{
    set(v.coordinate.x, v.coordinate.y, v.coordinate.z, v.coordinate.w);
}

Quaternion::Quaternion(const Matrix4& matrix)
{
    coordinate.w = sqrt(std::max(0.0f, 1.0f + matrix(0, 0) + matrix(1, 1) + matrix(2, 2)))/2.0f;
    coordinate.x = sqrt(std::max(0.0f, 1.0f + matrix(0, 0) - matrix(1, 1) - matrix(2, 2)))/2.0f;
    coordinate.y = sqrt(std::max(0.0f, 1.0f - matrix(0, 0) + matrix(1, 1) - matrix(2, 2)))/2.0f;
    coordinate.z = sqrt(std::max(0.0f, 1.0f - matrix(0, 0) - matrix(1, 1) + matrix(2, 2)))/2.0f;
    coordinate.x = COPYSIGN(coordinate.x, matrix(1, 2) - matrix(2, 1));
    coordinate.y = COPYSIGN(coordinate.y, matrix(2, 0) - matrix(0, 2));
    coordinate.z = COPYSIGN(coordinate.z, matrix(0, 1) - matrix(1, 0));
}

Quaternion::Quaternion(const Quaternion& quat)
    :   coordinate(quat.coordinate)
{
}

Quaternion& Quaternion::operator=(const Quaternion& quat)
{
    coordinate = quat.coordinate;
    return *this;
}

Quaternion& operator*=(Quaternion& lhs, const Quaternion& rhs)
{
    lhs = lhs * rhs;
    return lhs;
}

Quaternion& operator+=(Quaternion& lhs, const Quaternion& rhs)
{
    lhs.coordinate.x += rhs.coordinate.x;
    lhs.coordinate.y += rhs.coordinate.y;
    lhs.coordinate.z += rhs.coordinate.z;
    lhs.coordinate.w += rhs.coordinate.w;
    return lhs;
}

Quaternion& operator-=(Quaternion& lhs, const Quaternion& rhs)
{
    lhs.coordinate.x -= rhs.coordinate.x;
    lhs.coordinate.y -= rhs.coordinate.y;
    lhs.coordinate.z -= rhs.coordinate.z;
    lhs.coordinate.w -= rhs.coordinate.w;
    return lhs;
}
    
Quaternion& operator/=(Quaternion& lhs, const Quaternion& rhs)
{
    return lhs *= rhs.inverse();
}

Quaternion& operator*=(Quaternion& q, float f)
{
    q.coordinate.x *= f;
    q.coordinate.y *= f;
    q.coordinate.z *= f;
    q.coordinate.w *= f;
    return q;
}

Quaternion& operator/=(Quaternion& q, float f)
{
    q.coordinate.x /= f;
    q.coordinate.y /= f;
    q.coordinate.z /= f;
    q.coordinate.w /= f;
    return q;
}

bool operator==(const Quaternion& lhs, const Quaternion& rhs)
{
    return CMPF(lhs.coordinate.x, rhs.coordinate.x) &&
           CMPF(lhs.coordinate.y, rhs.coordinate.y) &&
           CMPF(lhs.coordinate.z, rhs.coordinate.z) &&
           CMPF(lhs.coordinate.w, rhs.coordinate.w);
}

Quaternion operator/(const Quaternion& lhs, const Quaternion& rhs)
{
    return lhs * rhs.inverse();
}

Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs)
{
    Quaternion result;
    result.coordinate.w = lhs.coordinate.w*rhs.coordinate.w - lhs.coordinate.x*rhs.coordinate.x - lhs.coordinate.y*rhs.coordinate.y - lhs.coordinate.z*rhs.coordinate.z;
    result.coordinate.x = lhs.coordinate.w*rhs.coordinate.x + lhs.coordinate.x*rhs.coordinate.w + lhs.coordinate.y*rhs.coordinate.z - lhs.coordinate.z*rhs.coordinate.y;
    result.coordinate.y = lhs.coordinate.w*rhs.coordinate.y - lhs.coordinate.x*rhs.coordinate.z + lhs.coordinate.y*rhs.coordinate.w + lhs.coordinate.z*rhs.coordinate.x;
    result.coordinate.z = lhs.coordinate.w*rhs.coordinate.z + lhs.coordinate.x*rhs.coordinate.y - lhs.coordinate.y*rhs.coordinate.x + lhs.coordinate.z*rhs.coordinate.w;
    return result;
}

Quaternion operator+(const Quaternion& lhs, const Quaternion& rhs)
{
    Quaternion result;
    result.coordinate.x = lhs.coordinate.x + rhs.coordinate.x;
    result.coordinate.y = lhs.coordinate.y + rhs.coordinate.y;
    result.coordinate.z = lhs.coordinate.z + rhs.coordinate.z;
    result.coordinate.w = lhs.coordinate.w + rhs.coordinate.w;
    return result;
}

Quaternion operator-(const Quaternion& lhs, const Quaternion& rhs)
{
    Quaternion result;
    result.coordinate.x = lhs.coordinate.x - rhs.coordinate.x;
    result.coordinate.y = lhs.coordinate.y - rhs.coordinate.y;
    result.coordinate.z = lhs.coordinate.z - rhs.coordinate.z;
    result.coordinate.w = lhs.coordinate.w - rhs.coordinate.w;
    return result;
}

Quaternion operator*(const Quaternion& q, float f)
{
    return Quaternion(q.coordinate.x*f, q.coordinate.y*f, q.coordinate.z*f, q.coordinate.w*f);
}

Quaternion operator/(const Quaternion& q, float f)
{
    return Quaternion(q.coordinate.x/f, q.coordinate.y/f, q.coordinate.z/f, q.coordinate.w/f);
}


Quaternion operator*(float f, const Quaternion& q)
{
    return Quaternion(q.coordinate.x*f, q.coordinate.y*f, q.coordinate.z*f, q.coordinate.w*f);
}

void Quaternion::identity()
{
    coordinate.x = coordinate.y = coordinate.z = 0.0f;
    coordinate.w = 1.0f;
}

Quaternion Quaternion::conjugate() const
{
    Quaternion result;
    result.coordinate.x = -coordinate.x;
    result.coordinate.y = -coordinate.y;
    result.coordinate.z = -coordinate.z;
    result.coordinate.w = coordinate.w;
    return result;
}

void Quaternion::conjugateSelf()
{
    coordinate.x = -coordinate.x;
    coordinate.y = -coordinate.y;
    coordinate.z = -coordinate.z;
}

Quaternion Quaternion::inverse() const
{
    return conjugate();
}

void Quaternion::invertSelf()
{
    conjugateSelf();
}

float Quaternion::length() const
{
    return sqrt(coordinate.x*coordinate.x + coordinate.y*coordinate.y + coordinate.z*coordinate.z + coordinate.w*coordinate.w);
}

void Quaternion::normalize()
{
    float l = length();
    if(l == 0)
        return;
    coordinate.x /= l;
    coordinate.y /= l;
    coordinate.z /= l;
    coordinate.w /= l;
}

void Quaternion::rotateX(float pitch)
{
    float   rx = sinf(pitch/2.0f),
            rw = cosf(pitch/2.0f);
    Quaternion tmp(*this);
    coordinate.w =  tmp.coordinate.w*rw - tmp.coordinate.x*rx;
    coordinate.x =  tmp.coordinate.w*rx + tmp.coordinate.x*rw;
    coordinate.y =  tmp.coordinate.y*rw + tmp.coordinate.z*rx;
    coordinate.z = -tmp.coordinate.y*rx + tmp.coordinate.z*rw;
    normalize();
}

void Quaternion::rotateY(float yaw)
{
    float   ry = sinf(yaw/2.0f),
            rw = cosf(yaw/2.0f);
    Quaternion tmp(*this);
    coordinate.w = tmp.coordinate.w*rw - tmp.coordinate.y*ry;
    coordinate.x = tmp.coordinate.x*rw - tmp.coordinate.z*ry;
    coordinate.y = tmp.coordinate.w*ry + tmp.coordinate.y*rw;
    coordinate.z = tmp.coordinate.x*ry + tmp.coordinate.z*rw;
    normalize();
}

void Quaternion::rotateZ(float roll)
{
    float   rz = sinf(roll/2.0f),
            rw = cosf(roll/2.0f);
    Quaternion tmp(*this);
    coordinate.w =  tmp.coordinate.w*rw - tmp.coordinate.z*rz;
    coordinate.x =  tmp.coordinate.x*rw + tmp.coordinate.y*rz;
    coordinate.y = -tmp.coordinate.x*rz + tmp.coordinate.y*rw;
    coordinate.z =  tmp.coordinate.w*rz + tmp.coordinate.z*rw;
    normalize();
}

void Quaternion::rotate(float angle, const Vector3& axis)
{
    Quaternion rotation;
    float sin_a = sinf(angle/2.0f);
    float cos_a = cosf(angle/2.0f);
    rotation.coordinate.x = axis.coordinate.x * sin_a;
    rotation.coordinate.y = axis.coordinate.y * sin_a;
    rotation.coordinate.z = axis.coordinate.z * sin_a;
    rotation.coordinate.w = cos_a;
    rotation.normalize();
    *this *= rotation;
}

float Quaternion::dot(const Quaternion& q) const
{
    return coordinate.x*q.coordinate.x + coordinate.y*q.coordinate.y + coordinate.z*q.coordinate.z + coordinate.w*q.coordinate.w;
}

Quaternion operator-(const Quaternion& q)
{
    return Quaternion(-q.coordinate.x, -q.coordinate.y, -q.coordinate.z, -q.coordinate.w);
}

Vector3 Quaternion::transform(const Vector3& v) const
{
    return v + 2.0f*vector().cross(vector().cross(v) + scalar()*v);
}

DualQuaternion::DualQuaternion()
{
    STATIC_ASSERT(sizeof(DualQuaternion) == 2*sizeof(Quaternion), "Quaternion has the wrong size");
}

DualQuaternion::DualQuaternion(const Quaternion& ndp, const Quaternion& dp)
    :   non_dual(ndp),
        dual(dp) {}

DualQuaternion::DualQuaternion(const DualQuaternion& d)
    :   non_dual(d.non_dual),
        dual(d.dual) {}

DualQuaternion::DualQuaternion(const Matrix4& mat)
    :   non_dual(mat),
        dual(mat[3][0]/2.0f, mat[3][1]/2.0f, mat[3][2]/2.0f, 0.0f)
{
    dual *= non_dual;
}

DualQuaternion& DualQuaternion::operator=(const DualQuaternion& d)
{
    non_dual = d.non_dual;
    dual = d.dual;
    return *this;
}

DualQuaternion& operator*=(DualQuaternion& lhs, const DualQuaternion& rhs)
{
    lhs.dual = lhs.dual*rhs.non_dual + lhs.non_dual*rhs.dual;
    lhs.non_dual *= rhs.non_dual;
    return lhs;
}

DualQuaternion& operator+=(DualQuaternion& lhs, const DualQuaternion& rhs)
{
    lhs.non_dual += rhs.non_dual;
    lhs.dual += rhs.dual;
    return lhs;
}

DualQuaternion& operator-=(DualQuaternion& lhs, const DualQuaternion& rhs)
{
    lhs.non_dual -= rhs.non_dual;
    lhs.dual -= rhs.dual;
    return lhs;
}

DualQuaternion& operator*=(DualQuaternion& d, float f)
{
    d.dual *= f;
    d.non_dual *= f;
    return d;
}

bool operator==(const DualQuaternion& lhs, const DualQuaternion& rhs)
{
    return lhs.non_dual == rhs.non_dual && lhs.dual == rhs.dual;
}

DualQuaternion operator*(const DualQuaternion& lhs, const DualQuaternion& rhs)
{
    return DualQuaternion(lhs.non_dual*rhs.non_dual, lhs.dual*rhs.non_dual + lhs.non_dual*rhs.dual);
}

DualQuaternion operator+(const DualQuaternion& lhs, const DualQuaternion& rhs)
{
    return DualQuaternion(lhs.non_dual + rhs.non_dual, lhs.dual + rhs.dual);
}

DualQuaternion operator-(const DualQuaternion& lhs, const DualQuaternion& rhs)
{
    return DualQuaternion(lhs.non_dual - rhs.non_dual, lhs.dual - rhs.dual);
}

DualQuaternion operator-(const DualQuaternion& q)
{
    return DualQuaternion(-q.non_dual, -q.dual);
}

DualQuaternion operator*(const DualQuaternion& d, float f)
{
    return DualQuaternion(d.non_dual*f, d.dual*f);
}

DualQuaternion operator*(float f, const DualQuaternion& d)
{
    return DualQuaternion(d.non_dual*f, d.dual*f);
}

DualQuaternion DualQuaternion::conjugate() const
{
    return DualQuaternion(non_dual, -dual);
}

void DualQuaternion::conjugateSelf()
{
    dual = -dual;
}

DualQuaternion DualQuaternion::conjugateQuaternion() const
{
    return DualQuaternion(non_dual.conjugate(), dual.conjugate());
}

void DualQuaternion::conjugateQuaternionSelf()
{
    non_dual.conjugateSelf();
    dual.conjugateSelf();
}

DualQuaternion DualQuaternion::inverse() const
{
    return conjugateQuaternion();
}

void DualQuaternion::invertSelf()
{
    non_dual = -non_dual.inverse();
    dual *= non_dual*non_dual;
}

float DualQuaternion::length() const
{
    float lnd = non_dual.length();
    return lnd + non_dual.dot(dual)/lnd;
}

void DualQuaternion::normalize()
{
    float lnd = non_dual.length();
    non_dual /= lnd, dual /= lnd;
}

void DualQuaternion::rotateX(float pitch)
{
    Quaternion qr;
    qr.identity();
    qr.rotateX(pitch);
    non_dual *= qr;
    dual *= qr;
}

void DualQuaternion::rotateY(float yaw)
{
    Quaternion qr;
    qr.identity();
    qr.rotateY(yaw);
    non_dual *= qr;
    dual *= qr;
}

void DualQuaternion::rotateZ(float roll)
{
    Quaternion qr;
    qr.identity();
    qr.rotateZ(roll);
    non_dual *= qr;
    dual *= qr;
}

void DualQuaternion::rotate(float angle, const Vector3& axis)
{
    Quaternion qr;
    qr.identity();
    qr.rotate(angle, axis);
    non_dual *= qr;
    dual *= qr;
}

void DualQuaternion::translate(const Vector3& vec)
{
    Quaternion qtr(vec.coordinate.x/2.0f, vec.coordinate.y/2.0f, vec.coordinate.z/2.0f, 0.0f);
    dual += non_dual*qtr;
}

void DualQuaternion::translateX(float x)
{
    Quaternion qtr(x/2.0f, 0.0f, 0.0f, 0.0f);
    dual += non_dual*qtr;
}

void DualQuaternion::translateY(float y)
{
    Quaternion qtr(0.0f, y/2.0f, 0.0f, 0.0f);
    dual += non_dual*qtr;
}

void DualQuaternion::translateZ(float z)
{
    Quaternion qtr(0.0f, 0.0f, z/2.0f, 0.0f);
    dual += non_dual*qtr;
}

Vector3 DualQuaternion::transform(const Vector3& v) const
{
    return non_dual.transform(v) + 2.0f*(non_dual.scalar()*dual.vector() -
                                         dual.scalar()*non_dual.vector() +
                                         non_dual.vector().cross(dual.vector()));
}

void DualQuaternion::identity()
{
    non_dual.identity();
    dual.coordinate.x = 0.0f;
    dual.coordinate.y = 0.0f;
    dual.coordinate.z = 0.0f;
    dual.coordinate.w = 0.0f;
}

DualQuaternion interpolate(float t, const DualQuaternion& q1, const DualQuaternion& q2)
{
    assert(0.0f <= t && t <= 1.0f);
    DualQuaternion qres((1.0f-t)*q1 + t*q2);
    qres.normalize();
    return qres;
}

float clampf(float val, float min, float max) 
{
    return val < min ? min :
            (val > max ? max : val);
}

int clampi(int val, int min, int max) 
{
    return val < min ? min :
            (val > max ? max : val);
}

std::ostream& operator<<(std::ostream& o, const Vector4& vec)
{
    o << '<' << vec.coordinate.x << ", " << vec.coordinate.y << ", " << vec.coordinate.z << ", " << vec.coordinate.w << '>';
    return o;
}

std::ostream& operator<<(std::ostream& o, const Vector3& vec)
{
    o << '<' << vec.coordinate.x << ", " << vec.coordinate.y << ", " << vec.coordinate.z << '>';
    return o;
}

std::ostream& operator<<(std::ostream& o, const Vector2& vec)
{
    o << '<' << vec.coordinate.x << ", " << vec.coordinate.y << '>';
    return o;
}

std::istream& operator>>(std::istream& i, Vector4& vec)
{
    i >> vec.coordinate.x >> vec.coordinate.y >> vec.coordinate.z >> vec.coordinate.w;
    return i;
}

std::istream& operator>>(std::istream& i, Vector3& vec)
{
    i >> vec.coordinate.x >> vec.coordinate.y >> vec.coordinate.z;
    return i;
}

std::istream& operator>>(std::istream& i, Vector2& vec)
{
    i >> vec.coordinate.x >> vec.coordinate.y;
    return i;
}

std::ostream& operator<<(std::ostream& o, const Matrix4& mat)
{
    for(int i = 0; i < 4; ++i) {
        o << "| ";
        for(int j = 0; j < 4; ++j)
            o << std::setw(6) << std::setprecision(3) << mat[j][i] << " ";
        o << "|\n";
    }
    return o;
}

std::istream& operator>>(std::istream& i, Matrix4& mat)
{
    i >> mat(0, 0) >> mat(1, 0) >> mat(2, 0) >> mat(3, 0)
      >> mat(0, 1) >> mat(1, 1) >> mat(2, 1) >> mat(3, 1)
      >> mat(0, 2) >> mat(1, 2) >> mat(2, 2) >> mat(3, 2)
      >> mat(0, 3) >> mat(1, 3) >> mat(2, 3) >> mat(3, 3);
    return i;
}

std::ostream& operator<<(std::ostream& o, const Quaternion& quat)
{
    return o << "[" << quat.coordinate.w << ", (" << quat.coordinate.x << ", " << quat.coordinate.y << ", " << quat.coordinate.z << ")]";
}

std::istream& operator>>(std::istream& i, Quaternion& quat)
{
    return i >> quat.coordinate.w >> quat.coordinate.x >> quat.coordinate.y >> quat.coordinate.z;
}

std::ostream& operator<<(std::ostream& o, const DualQuaternion& dq)
{
    return o << "| " << dq.non_dual << " |\n"
             << "| " << dq.dual << " |";
}

std::istream& operator>>(std::istream& i, DualQuaternion& dq)
{
    return i >> dq.non_dual >> dq.dual;
}
}
